/*
* avr_test.c
*
* Created: 3/26/2011 22:35:52
*  Author: morpheus
*/ 

#include <stdlib.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>

#ifndef F_CPU
#define F_CPU 16000000UL
#endif
#include <util/delay.h>

#include "global.h"
#include "uart.h"
#include "atmega_adc.h"
#include "servo.h"
#include "sabertooth.h"

#define AREF 5
#define MAXARRAY 5

//uint16_t getPulseWidth();
//unsigned long pulseIe(uint8_t pin, uint8_t state, unsigned long timeout);


/*****************************************
* Integer compareison:
*   returns negative if b>a and positive
*   if a > b
*
*****************************************/
int int_cmp(const void *a, const void *b)
{
  return *(int *)a - *(int *)b;
}
uint8_t checkDistance(uint8_t channel)
{
  uint8_t count, x, y, holder;
  uint8_t distance0[MAXARRAY];
  uint8_t distance_len;

  // get a couple of readings
  for(count = 0; count < MAXARRAY+1; ++count)
  {
    distance0[count] = readADC(channel) * AREF / 10;
    //_delay_ms(1);
  } 

  /* QSORT */
  //distance_len = sizeof(distance0)/sizeof(uint8_t); 
  //qsort(distance0, distance_len, sizeof(uint8_t), int_cmp);

  /* Bubble sort */
  for(x = 0; x < MAXARRAY; x++)
  {
    for(y = 0; y < MAXARRAY - 1; y++)
    {
      holder = distance0[y+1];
      distance0[y+1] = distance0[y];
      distance0[y] = holder;
    }
  }

  // filter crazy signals average 4
  // scanner front
  /*for(count = 0; count < 3; ++count)
  {
    distance += readADC(channel) * AREF / 10;
    //distance += readADC(channel);
    _delay_ms(1);
  }

  return  distance / count;*/
  return distance0[2];
}

int main(void)
{

  uint8_t count, x, y, holder, temp;
  uint8_t distance0[MAXARRAY];
  uint8_t distance_len;

  uint8_t distance1 = 0;
	char buffer0[20];
	//char buffer1[5];
	//char buffer3[5];
  //uint8_t angle = 0;
  BIT_up = true;// servo direction flag
  uint8_t distance;

  ServoState servoState;
  servoState.pin = PB2;
  servoState.angle = 90;

  uart_init(BAUD_RATE);

  //_delay_ms(1000);
	//uart_puts("Initalize\n\r");

  // Initalize the servo
  initServo();
  attachServo(&servoState);

  all_stop();

	//ADC/
	initADC();
  readADC(0); // calibration of sensor
  _delay_ms(250);
  //readADC(1); // calibration of sensor
  _delay_ms(250);
  
  _delay_ms(1000);

  while(1)
  {

    // debugging
    //distance0 = checkDistance(0);;kh
    //distance0 = readADC(0) * AREF / 10;
    //distance0 = readADC(0);
    //distance = checkDistance(0);
    // get a couple of readings
    for(count = 0; count < MAXARRAY+1; ++count)
    {
      holder = readADC(0) *AREF / 10;
      if(count != 0)
      {
        // TODO perform simple sorting
        if(holder > distance0[count - 1])
        {
          temp = distance0[count -1];
          distance0[count -1] = holder;
          distance0[count] = temp;
        }
        else
        {
          distance0[count] = holder;
        }

      }
      else
      {
        distance0[count] = holder;
      }
      //distance0[count] = readADC(0) * AREF / 10;
      //_delay_ms(1);
    } 

    distance = distance0[MAXARRAY];

    sprintf(buffer0,"%d#%d#%d\n", distance, distance1, servoState.angle);
    uart_puts(buffer0);
    
    /*if(distance < 10)
    {
      all_stop();
    }
    else*/ if(distance > 10)
    {
        //sprintf(buffer0,"%d#%d#%d\n", distance0, distance1, servoState.angle);
      uart_puts("forward\n");
     // uart_puts(buffer0);
      //_delay_ms(500);

      // forward
      //all_stop();
	    forward(HALF);
    }
    else if( (distance < 15) && (servoState.angle >= 60 && servoState.angle <= 90) )
    {
        //sprintf(buffer0,"%d#%d#%d\n", distance0, distance1, servoState.angle);
      uart_puts("left\n");
      //  uart_puts(buffer0);
      // left
      //_delay_ms(500);
      all_stop();
        turnLeft();
      all_stop();

    }
    else if( (distance < 15) && (servoState.angle > 90 && servoState.angle <= 140) )
    {
      uart_puts("right\n");
       // uart_puts(buffer0);
      // right
      //_delay_ms(500);
      all_stop();
        turnRight();
      all_stop();
    }

    // filter crazy signals average 4.
    // split the scans up so we dont mix the
    // adc's
    // scanner rear
    /*for(count = 0; count < 4; ++count)
    {
      distance1 += readADC(1) * AREF / 10;
      _delay_ms(1);
    }
    distance1 = distance1 / count;*/

    write_deg(&servoState);
    // Servo sweep direction
    if(BIT_up)
    {
      servoState.angle += 2;
    }
    else
    {
      servoState.angle -= 2;
    }
  
    if(servoState.angle == 140)
    {
      BIT_up = false;
    }
    else if(servoState.angle == 50)
    {
      BIT_up = true;
    }

	  //_delay_ms(50);
  }
}
